# encoding=utf-8
import numpy as np
import matplotlib.pyplot as plt

# 创建一个0-99的一维矩阵
z = [i for i in range(100)]
z_watch = np.mat(z)
#print (z_watch)

# 创建一个方差为1的高斯噪声，精确到小数点后两位
noise = np.round(np.random.normal(0, 1, 100), 2)
#print(noise)
noise_mat = np.mat(noise)
#print(noise_mat)

# 将z的观测值和噪声相加
z_mat = z_watch + noise_mat
#print(z_mat)


# 定义x的初始状态
x_mat = np.mat([[0, ], [0, ]])
print("x_mat:",x_mat)

# 定义初始状态协方差矩阵
p_mat = np.mat([[1, 0], [0, 1]])
print("p_mat:",p_mat)

# 定义状态转移矩阵，因为每秒钟采一次样，所以delta_t = 1
f_mat = np.mat([[1, 1], [0, 1]])
print("f_mat:",f_mat)

# 定义状态转移协方差矩阵，这里我们把协方差设置的很小，因为觉得状态转移矩阵准确度高
q_mat = np.mat([[0.0001, 0], [0, 0.0001]])
print("q_mat:",q_mat)

# 定义观测矩阵
h_mat = np.mat([1, 0])
print("h_mat:",h_mat)

# 定义观测噪声协方差
r_mat = np.mat([1])
print("r_mat:",r_mat)

#依次对应于卡尔曼滤波的5个公式，迭代100次
#前两个表示的是根据上一时刻的状态来预测当前时刻的状态，通过这两个公式我们得到的是非最佳估计的x和P
# 后三个公式就是通过当前的观测值来更新x和P，更新之后的就是最佳观测值
for i in range(100):
    #x的初始状态乘以状态转移矩阵，使原始矩阵的状态发生转移
    x_predict = f_mat * x_mat
    #噪声协方差矩阵P乘以状态转移矩阵F，加上表示误差的矩阵Q，
    # 得到权衡预测状态协方差矩阵P，表示不确定性在各个时刻之间的传递关系
    p_predict = f_mat * p_mat * f_mat.T + q_mat
    #求出卡尔曼系数，R为观测噪声的协方差矩阵，H为观测矩阵
    kalman = p_predict * h_mat.T / (h_mat * p_predict * h_mat.T + r_mat)
    #修正预测值
    x_mat = x_predict + kalman * (z_mat[0, i] - h_mat * x_predict)
    #对噪声协方差进行更新
    p_mat = (np.eye(2) - kalman * h_mat) * p_predict
    plt.plot(x_mat[0, 0], x_mat[1, 0], 'ro', markersize=1)

plt.show()






